﻿import rospy
from geometry_msgs.msg import PoseStamped
import paho.mqtt.client as mqtt
import json
from std_msgs.msg import String

client = mqtt.Client()

def on_connect(client, userdata, flags, rc):
    if rc == 0:
        print("Connected to MQTT Broker!")
        client.subscribe('mqtt/test_server')

def on_message(client, userdata, msg):
    global control_info_pub
    print("Received %s", msg.payload.decode())
    control_info_pub.publish(msg.payload.decode())
    rospy.loginfo("Publish cloud_control message %s",
                      msg.payload.decode())

def poseCallback(msg):
    global client
    client.publish('mqtt/test_client', payload=json.dumps({'x': msg.pose.position.x, 'y': msg.pose.position.y}), qos=0)

if __name__ == '__main__':
    client.on_connect = on_connect
    client.on_message = on_message
    client.username_pw_set("test", "test")
    client.connect('m1012864.cn-shenzhen.emqx.cloud', 11192)

    rospy.init_node('pos_subscriber',anonymous=True)
    rospy.Subscriber("/laneline_info",PoseStamped,poseCallback)
    global control_info_pub
    control_info_pub = rospy.Publisher(
        '/control_info', String, queue_size=10)
    client.loop_forever()

    
    